{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import linalg as LA\n",
    "from numpy.linalg import inv\n",
    "import math\n",
    "\n",
    "import matplotlib as mpl\n",
    "from matplotlib import pyplot as plt\n",
    "from matplotlib import cm\n",
    "import matplotlib.patches as mpatches\n",
    "\n",
    "from sklearn import metrics\n",
    "from sklearn.metrics import average_precision_score\n",
    "from sklearn.metrics import precision_recall_curve\n",
    "\n",
    "# Load data data\n",
    "ground_truth_poses = np.loadtxt(\"../data/experiments/pose_scanner_leica_affine_apartment.txt\") # id,x,y,z,w,x,y,z\n",
    "ground_truth_poses = np.array([ground_truth_poses]).T        \n",
    "\n",
    "corrected_poses = np.loadtxt(\"../data/experiments/corrected_poses_360fov_apartment_1200_run1.txt\")              # idA,idB,x,y,z,w,x,y,z\n",
    "corrected_poses = np.array([corrected_poses]).T\n",
    "\n",
    "results = np.loadtxt(\"../data/experiments/compare_results_360fov_apartment_1200_run1.txt\")                     # idA,idB,\n",
    "results = np.array([results]).T                                                     # fov_overlap, octree_overlap\n",
    "                                                                                    # alignability, alignment_risk\n",
    "                                                                                    # degeneracy, ICN\n",
    "# ground_truth_poses\n",
    "# print results[5]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Compute translation error\n",
    "def translationError(xg, yg, zg, xc, yc, zc):\n",
    "   \n",
    "    trans_err = np.zeros((xc.shape[0],1))\n",
    "    gt_index = 0\n",
    "    for i in range(0, xc.shape[0]):\n",
    "        if (gt_index > xg.shape[0]-1):\n",
    "            gt_index = 0\n",
    "        \n",
    "#         print gt_index, i\n",
    "  \n",
    "        trans_err[i] =  math.sqrt( pow((xg[gt_index] - xc[i]),2.0) + \n",
    "                                   pow((yg[gt_index] - yc[i]),2.0) +\n",
    "                                   pow((zg[gt_index] - zc[i]),2.0) );\n",
    "        gt_index = gt_index+1\n",
    "    return trans_err\n",
    "\n",
    "transl_err = translationError(ground_truth_poses[1], # [col][row]\n",
    "                              ground_truth_poses[2],\n",
    "                              ground_truth_poses[3],\n",
    "                              corrected_poses[2],\n",
    "                              corrected_poses[3],\n",
    "                              corrected_poses[4])\n",
    "\n",
    "# transl_err"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Compute rotation error\n",
    "def quaternion_matrix(quaternion):\n",
    "    quaternion = quaternion / LA.norm(quaternion)\n",
    "#     print quaternion\n",
    "    w = quaternion[0]\n",
    "    x = quaternion[1]\n",
    "    y = quaternion[2]\n",
    "    z = quaternion[3]\n",
    "    \n",
    "    tmp = np.array([[1 - 2*y*y - 2*z*z,   2*x*y - 2*z*w,       2*x*z + 2*y*w    ],\n",
    "                    [2*x*y + 2*z*w,       1 - 2*x*x - 2*z*z,   2*y*z - 2*x*w    ],\n",
    "                    [2*x*z - 2*y*w,       2*y*z + 2*x*w,       1 - 2*x*x - 2*y*y]])\n",
    "    result = tmp[:, :, 0]\n",
    "    return result\n",
    "\n",
    "def rotationError3D(q1g, q2g, q3g, q4g, q1c, q2c, q3c, q4c):\n",
    "    rot_err = np.zeros((q1c.shape[0],1))\n",
    "#     print rot_err.shape\n",
    "    gt_index = 0\n",
    "    for i in range(0, q1c.shape[0]):\n",
    "        if (gt_index > q1g.shape[0]-1):\n",
    "            gt_index = 0\n",
    "            \n",
    "#         print gt_index, 1\n",
    "        rot_g = quaternion_matrix([q1g[gt_index], q2g[gt_index], q3g[gt_index], q4g[gt_index]])\n",
    "#         print rot_g\n",
    "#         print q1g[gt_index]\n",
    "        rot_c = quaternion_matrix([q1c[i], q2c[i], q3c[i], q4c[i]])\n",
    "        rot_g_inv = inv(rot_g)\n",
    "        delta_rot = rot_c * rot_g_inv\n",
    "        trace_rot = np.trace( delta_rot )\n",
    "#         print trace_rot\n",
    "  \n",
    "        rot_err[i] = np.arccos ( (trace_rot-1)/2 ) * 180.0 / math.pi;\n",
    "        gt_index = gt_index+1\n",
    "    return rot_err\n",
    "\n",
    "rot_err = rotationError3D(ground_truth_poses[4], # [col][row]\n",
    "                          ground_truth_poses[5],\n",
    "                          ground_truth_poses[6],\n",
    "                          ground_truth_poses[7],\n",
    "                          corrected_poses[5],\n",
    "                          corrected_poses[6],\n",
    "                          corrected_poses[7],\n",
    "                          corrected_poses[8])\n",
    "\n",
    "# print rot_err"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.],\n",
       "       [ 0.],\n",
       "       [ 0.],\n",
       "       ..., \n",
       "       [ 1.],\n",
       "       [ 0.],\n",
       "       [ 0.]])"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Create Labels given ground truth and errors\n",
    "transl_thresh = 0.02 # [m]\n",
    "rot_thresh = 30\n",
    "\n",
    "def createLabels(transl_err, rot_err):\n",
    "    \n",
    "    labels = np.zeros((transl_err.shape[0],1))\n",
    "\n",
    "    for i in range(0, labels.shape[0]):\n",
    "        if transl_err[i] > transl_thresh: #and rot_err[i] > rot_thresh:\n",
    "            labels[i] = 1.0\n",
    "\n",
    "    return labels\n",
    "\n",
    "labels = createLabels(transl_err, rot_err)\n",
    "\n",
    "labels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.text.Text at 0x7f2d564b9c10>"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "probs_ours = results[5]\n",
    "probs_degeneracy = results[6]\n",
    "probs_icn = results[7]\n",
    "\n",
    "# ROC Curve and cut-off point\n",
    "fpr_ours, tpr_ours, threshold_ours = metrics.roc_curve(labels, probs_ours, pos_label=1)\n",
    "roc_ours_auc = metrics.auc(fpr_ours, tpr_ours)\n",
    "\n",
    "fpr_degeneracy, tpr_degeneracy, threshold_degeneracy = metrics.roc_curve(labels, probs_degeneracy, pos_label=1)\n",
    "roc_degeneracy_auc = metrics.auc(fpr_degeneracy, tpr_degeneracy)\n",
    "\n",
    "fpr_icn, tpr_icn, threshold_icn = metrics.roc_curve(labels, probs_icn, pos_label=1)\n",
    "roc_icn_auc = metrics.auc(fpr_icn, tpr_icn)\n",
    "\n",
    "# Plot ROC Curve\n",
    "plt.title('ROC for Failure Prediction using Alignment Risk')\n",
    "plt.plot(fpr_ours, tpr_ours, 'r', linewidth=2.0, label = 'Ours AUC = %0.2f' % roc_ours_auc)\n",
    "# plt.plot(fpr_degeneracy, tpr_degeneracy, 'b', linewidth=2.0, label = 'Degeneracy AUC = %0.2f' % roc_degeneracy_auc)\n",
    "# plt.plot(fpr_icn, tpr_icn, 'g', linewidth=2.0, label = 'ICN AUC = %0.2f' % roc_icn_auc)\n",
    "plt.legend(loc = 'lower right')\n",
    "plt.plot([0, 1], [0, 1],'r--')\n",
    "plt.xlim([0, 1])\n",
    "plt.ylim([0, 1])\n",
    "plt.ylabel('True Positive Rate')\n",
    "plt.xlabel('False Positive Rate')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Average precision-recall score: 0.86\n"
     ]
    }
   ],
   "source": [
    "# Precision-Recall Score\n",
    "average_precision = average_precision_score(labels, probs_ours)\n",
    "\n",
    "print('Average precision-recall score: {0:0.2f}'.format(\n",
    "      average_precision))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.text.Text at 0x7f2d53c09210>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Plot Precision-Recall Curve\n",
    "fig = plt.figure()\n",
    "precision, recall, _ = precision_recall_curve(labels, probs_ours, pos_label=1)\n",
    "\n",
    "plt.step(recall, precision, color='r',\n",
    "         where='post', linewidth=2.0, label = 'Ours AUC = %0.2f' % average_precision)\n",
    "plt.legend(loc = 'lower right')\n",
    "# plt.fill_between(recall, precision, step='post', alpha=0.2,\n",
    "#                  color='b')\n",
    "plt.xlabel('Recall')\n",
    "plt.ylabel('Precision')\n",
    "plt.ylim([0.0, 1.05])\n",
    "plt.xlim([0.0, 1.0])\n",
    "plt.title('Precision-Recall for Failure Prediction using Alignment Risk')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "risk_threshold = 0.50\n",
    "\n",
    "# Fill error matrix\n",
    "def samplemat(dims, labels, probs_ours):\n",
    "    matrix = np.ones(dims)\n",
    "    line_count = 0\n",
    "    for i in range(min(dims)):\n",
    "        for j in range(min(dims)):\n",
    "#             print labels[line_count], probs_ours[line_count]\n",
    "            if (labels[line_count]==1.0 and probs_ours[line_count]>risk_threshold):\n",
    "                matrix[i, j] = 1.0 # true positive (dark green)\n",
    "            elif (labels[line_count]==0.0 and probs_ours[line_count]<=risk_threshold):\n",
    "                matrix[i, j] = 0.7 # true negative (light green)\n",
    "            elif (labels[line_count]==0.0 and probs_ours[line_count]>risk_threshold):\n",
    "                matrix[i, j] = 0.0 # false positive (dark red)\n",
    "            else:\n",
    "                matrix[i, j] = 0.1 # false negative (light red)\n",
    "            line_count = line_count + 1\n",
    "    return matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#####################################\n",
    "# Plot Failure Prediction Map (ours)\n",
    "#####################################\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(111)\n",
    "cmap = cm.get_cmap('RdYlGn')\n",
    "\n",
    "ax.set_xlabel('Cloud A')\n",
    "ax.set_ylabel('Cloud B')\n",
    "ax.xaxis.set_label_position('top')\n",
    "# ax.text(15, 32, 'Failure Predictions using Alignment Risk', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "# ax.text(15, 33.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "ax.text(23, 47, 'Failure Predictions using Alignment Risk', color='black', horizontalalignment='center',\n",
    "         fontsize=15)\n",
    "# ax.text(23, 48.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "\n",
    "# ax.text(0, 33, 'True Positives', color=cmap(1.0), horizontalalignment='center',\n",
    "#          fontsize=11, weight='bold')\n",
    "# ax.text(4, 33, 'True Negatives', color=cmap(0.7), horizontalalignment='center',\n",
    "#          fontsize=11, weight='bold')\n",
    "\n",
    "# Display matrix\n",
    "mat_size = int(math.sqrt(labels.shape[0]))\n",
    "# pass octree-overlap column to be taken from results\n",
    "matrix = samplemat((mat_size, mat_size), labels, probs_ours)\n",
    "cax = ax.matshow(matrix, cmap=cmap)\n",
    "# ax.text(23, 1.5, 'TP', color=cmap(1.0), bbox={'facecolor': 'white', 'pad': 10})\n",
    "\n",
    "# Minor ticks\n",
    "ax.set_xticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "ax.set_yticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "\n",
    "# Gridlines based on minor ticks\n",
    "# ax.grid(which='minor', color='k')\n",
    "\n",
    "# print threshold_ours\n",
    "# plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "45\n"
     ]
    }
   ],
   "source": [
    "# deg_threshold = 0.055 #stairs\n",
    "deg_threshold = 0.03 #apartment\n",
    "\n",
    "# Fill error matrix\n",
    "def samplematDeg(dims, labels, probs_degeneracy, results):\n",
    "    matrix = np.ones(dims)\n",
    "    line_count = 0\n",
    "    for i in range(min(dims)):\n",
    "        for j in range(min(dims)):\n",
    "#             print labels[line_count], probs_ours[line_count]\n",
    "#             print i, j, results[0][line_count], results[1][line_count]\n",
    "            if (labels[line_count]==1.0 and probs_degeneracy[line_count]<deg_threshold):\n",
    "                matrix[i, j] = 1.0 # true positive (dark green)\n",
    "            elif (labels[line_count]==0.0 and probs_degeneracy[line_count]>=deg_threshold):\n",
    "                matrix[i, j] = 0.7 # true negative (light green)\n",
    "            elif (labels[line_count]==0.0 and probs_degeneracy[line_count]<deg_threshold):\n",
    "                matrix[i, j] = 0.0 # false positive (dark red)\n",
    "            else:\n",
    "                matrix[i, j] = 0.1 # false negative (light red)\n",
    "            line_count = line_count + 1\n",
    "    return matrix\n",
    "\n",
    "# print probs_degeneracy\n",
    "##########################################\n",
    "# Plot Failure Prediction Map (degeneracy)\n",
    "##########################################\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(111)\n",
    "cmap = cm.get_cmap('RdYlGn')\n",
    "\n",
    "ax.set_xlabel('Cloud A')\n",
    "ax.set_ylabel('Cloud B')\n",
    "ax.xaxis.set_label_position('top')\n",
    "# ax.text(15, 32, 'Failure Predictions using Degeneracy', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "# ax.text(15, 33.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "ax.text(23, 47, 'Failure Predictions using Degeneracy', color='black', horizontalalignment='center',\n",
    "         fontsize=15)\n",
    "# ax.text(23, 48.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "\n",
    "# Display matrix\n",
    "mat_size = int(math.sqrt(labels.shape[0]))\n",
    "print mat_size\n",
    "# pass octree-overlap column to be taken from results\n",
    "matrix = samplematDeg((mat_size, mat_size), labels, probs_degeneracy, results)\n",
    "cax = ax.matshow(matrix, cmap=cmap)\n",
    "# ax.text(23, 1.5, 'TP', color=cmap(1.0), bbox={'facecolor': 'white', 'pad': 10})\n",
    "\n",
    "# Minor ticks\n",
    "ax.set_xticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "ax.set_yticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "\n",
    "# Gridlines based on minor ticks\n",
    "# ax.grid(which='minor', color='k')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.0677487 ]\n",
      " [ 0.100438  ]\n",
      " [ 0.074081  ]\n",
      " ..., \n",
      " [ 0.00385641]\n",
      " [ 0.0507465 ]\n",
      " [ 0.026204  ]]\n",
      "45\n"
     ]
    }
   ],
   "source": [
    "# icn_threshold = 0.03 #stairs\n",
    "icn_threshold = 0.05 #apartment\n",
    "\n",
    "# Fill error matrix\n",
    "def samplematICN(dims, labels, probs_icn, results):\n",
    "    matrix = np.ones(dims)\n",
    "    line_count = 0\n",
    "    for i in range(min(dims)):\n",
    "        for j in range(min(dims)):\n",
    "#             print labels[line_count], probs_ours[line_count]\n",
    "#             print i, j, results[0][line_count], results[1][line_count]\n",
    "            if (labels[line_count]==1.0 and probs_icn[line_count]<icn_threshold):\n",
    "                matrix[i, j] = 1.0 # true positive (dark green)\n",
    "            elif (labels[line_count]==0.0 and probs_icn[line_count]>=icn_threshold):\n",
    "                matrix[i, j] = 0.7 # true negative (light green)\n",
    "            elif (labels[line_count]==0.0 and probs_icn[line_count]<icn_threshold):\n",
    "                matrix[i, j] = 0.0 # false positive (dark red)\n",
    "            else:\n",
    "                matrix[i, j] = 0.1 # false negative (light red)\n",
    "            line_count = line_count + 1\n",
    "    return matrix\n",
    "\n",
    "print probs_icn\n",
    "##########################################\n",
    "# Plot Failure Prediction Map (ICN)\n",
    "##########################################\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(111)\n",
    "cmap = cm.get_cmap('RdYlGn')\n",
    "\n",
    "ax.set_xlabel('Cloud A')\n",
    "ax.set_ylabel('Cloud B')\n",
    "ax.xaxis.set_label_position('top')\n",
    "# ax.text(15, 32, 'Failure Predictions using ICN', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "# ax.text(15, 33.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "ax.text(23, 47, 'Failure Predictions using ICN', color='black', horizontalalignment='center',\n",
    "         fontsize=15)\n",
    "# ax.text(23, 48.5, 'TP: dark green, TN: light green, FP: dark red, FN: light red', color='black', horizontalalignment='center',\n",
    "#          fontsize=15)\n",
    "\n",
    "# Display matrix\n",
    "mat_size = int(math.sqrt(labels.shape[0]))\n",
    "print mat_size\n",
    "# pass octree-overlap column to be taken from results\n",
    "matrix = samplematICN((mat_size, mat_size), labels, probs_icn, results)\n",
    "cax = ax.matshow(matrix, cmap=cmap)\n",
    "# ax.text(23, 1.5, 'TP', color=cmap(1.0), bbox={'facecolor': 'white', 'pad': 10})\n",
    "\n",
    "# Minor ticks\n",
    "ax.set_xticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "ax.set_yticks(np.arange(-.5, mat_size-1, 1), minor=True);\n",
    "\n",
    "# Gridlines based on minor ticks\n",
    "# ax.grid(which='minor', color='k')\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
